/*
 * File - ros_slam_publisher.cpp
 *
 * This file is part of the Inuitive SDK
 *
 * Copyright (C) 2014-2020 Inuitive All Rights Reserved
 *
 */

/**
 * \cond INTERNAL
 */

#include "config.h"

#include <fstream>
#include <iostream>
#include <nodelet/nodelet.h>
#include <ros/rate.h>
#include <ros/ros.h>
#include <sensor_msgs/distortion_models.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>

/**
 * \endcond
 */

#include "ros_slam_publisher.h"

#include "inudev_ros_nodelet.h"

/**
 * \file ros_slam_publisher.cpp
 *
 * \brief CRosSlamPublisher
 */

/**
 * \cond INTERNAL
 */

static const char rcsid[]="$Id: 05f1828af638ff499e76297e2dde6b15be9efdff $";

/**
 * \endcond
 */

namespace __INUROS__NAMESPACE__
{
    std::vector<const char *> CRosSlamPublisher::fieldNames = { "x", "y", "z" };

    CRosSlamPublisher::CRosSlamPublisher(ros::NodeHandle& _node,  std::shared_ptr<image_transport::ImageTransport> _image_transport, CRosSensor* _rosSensor, std::string iTopicName)
        : CRosPublisher(_node, _image_transport, "nav_msgs", _rosSensor)
        , publisher(node.advertise<nav_msgs::Path>(iTopicName, 1, false))
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());
        path.header.stamp = ros::Time::now();
        path.header.frame_id = "map";
        mPathSize = -1;
    }

    void CRosSlamPublisher::FrameCallback(std::shared_ptr<InuDev::CSlamStream> iStream, std::shared_ptr<const InuDev::CSlamFrame> iFrame, InuDev::CInuError iError)
    {
        NODELET_DEBUG_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        if (!CheckFrame(iFrame, iError))
        {
            return;
        }
        
               
        std::array<float, 4> Q;
        std::array<float, 3> T;
        
        iFrame->ConvertPose4x4ToQuaternionTranslation(
                iFrame->mPose4x4BodyToWorld,
                Q,
                T
        );

        poseT iPose, oPose;
        iPose.mQuaternion[0] = Q[0];
        iPose.mQuaternion[1] = Q[1];
        iPose.mQuaternion[2] = Q[2];
        iPose.mQuaternion[3] = Q[3];
        iPose.mTranslation[0] = T[0];
        iPose.mTranslation[1] = T[1];
        iPose.mTranslation[2] = T[2];

        Inverse_pos(iPose, oPose);

        geometry_msgs::PoseStamped poseStamped;

        poseStamped.pose.position.x = oPose.mTranslation[0];
        poseStamped.pose.position.y = oPose.mTranslation[1];
        poseStamped.pose.position.z = oPose.mTranslation[2];
        poseStamped.pose.orientation.x = oPose.mQuaternion[0];
        poseStamped.pose.orientation.y = oPose.mQuaternion[1];
        poseStamped.pose.orientation.z = oPose.mQuaternion[2];
        poseStamped.pose.orientation.w = oPose.mQuaternion[3];

        poseStamped.header.stamp = ros::Time::now();
        poseStamped.header.frame_id = "map";
        poseStamped.header.seq = iFrame->FrameIndex;

        if(path.poses.size() == mPathSize)
        {
            path.poses.erase(path.poses.begin());
        }

        path.poses.push_back(poseStamped);
        publisher.publish(path);

    }

    InuDev::CInuError CRosSlamPublisher::RegisterCallback()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        InuDev::CSlamStream::CallbackFunction callback = std::bind(&CRosSlamPublisher::FrameCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);

        return std::static_pointer_cast<InuDev::CSlamStream>(stream)->Register(callback);
    }

    InuDev::CInuError CRosSlamPublisher::UnregisterCallback()
    {
        return std::static_pointer_cast<InuDev::CSlamStream>(stream)->Register(nullptr);
    }

    int CRosSlamPublisher::GetNumSubscribers()
    {
        publisherPrev = publisherCurr;
        publisherCurr = publisher.getNumSubscribers();

        return publisherCurr;
    }

    InuDev::CInuError CRosSlamPublisher::InitStream()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        InuDev::CInuError err = std::static_pointer_cast<InuDev::CSlamStream>(stream)->Init();

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        return InuDev::EErrorCode::eOK;
    }

    InuDev::CInuError CRosSlamPublisher::StartStream()
    {
        stream = std::static_pointer_cast<InuDev::CBaseStream>(sensor->getSensor()->CreateSlamStream());
        NODELET_ERROR_STREAM_COND(!stream, __INUROS_FUNCTION_NAME__ << ": failed creating " << getName());

        InuDev::EErrorCode err = CRosPublisher::StartStream();

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        mPathSize = CInuDevRosNodelet::config.SLAM_Vector_Size;
        if (!path.poses.empty())
        {
            path.poses.clear();
        }

        return InuDev::EErrorCode::eOK;
    }

    void CRosSlamPublisher::Inverse_pos(poseT &iPose, poseT &oPose)
    {
        int k;
        for(k = 0 ; k < 3; k++)
        {
            oPose.mQuaternion[k] = - iPose.mQuaternion[k];
            oPose.mTranslation[k] = - iPose.mTranslation[k];
        }
        oPose.mQuaternion[3] = iPose.mQuaternion[3];

        float newTrans[3];
        MultiplyQuaternionByVec(oPose, oPose.mTranslation, newTrans);
        oPose.mTranslation[0] = newTrans[0];
        oPose.mTranslation[1] = newTrans[1];
        oPose.mTranslation[2] = newTrans[2];

    }
    void CRosSlamPublisher::MultiplyQuaternionByVec(poseT& oPose, float v[3], float newTrans[3])
    {
        float quaternion[4];
        quaternion[0] = oPose.mQuaternion[0];
        quaternion[1] = oPose.mQuaternion[1];
        quaternion[2] = oPose.mQuaternion[2];
        quaternion[3] = oPose.mQuaternion[2];

        float uv[3];
        uv[0] = 2 * (quaternion[1] * v[2] - quaternion[2] * v[1]);
        uv[1] = 2 * (quaternion[2] * v[0] - quaternion[0] * v[2]);
        uv[2] = 2 * (quaternion[0] * v[1] - quaternion[1] * v[0]);
        newTrans[0] = v[0] + quaternion[3] * uv[0] + (quaternion[1] * uv[2] - quaternion[2] * uv[1]);
        newTrans[1] = v[1] + quaternion[3] * uv[1] + (quaternion[2] * uv[0] - quaternion[0] * uv[2]);
        newTrans[2] = v[2] + quaternion[3] * uv[2] + (quaternion[0] * uv[1] - quaternion[1] * uv[0]);
    }
}
